#include <WPILib.h>
#include <memory>
#include <cassert>
#include <iomanip>
#include "PS3Controller.h"

#define ENABLE_TANK_MODE

#define LEFT_JOYSTICK_PORT    1
#define RIGHT_JOYSTICK_PORT   2

class Robot: public IterativeRobot
{
private:
	std::auto_ptr<RobotDrive> myRobot;
	std::auto_ptr<Joystick> stick0;
	std::auto_ptr<Joystick> leftStick;
	std::auto_ptr<Joystick> rightStick;
	std::auto_ptr<PS3Controller> controller;
	std::auto_ptr< ControllerMonitor<PS3Controller> > monitor;
	LiveWindow* lw;
	int autoLoopCounter;
	bool forward;

public:
	Robot()
		: lw(0), autoLoopCounter(0), forward(0)
		{}

private:
	void RobotInit()
	{
		myRobot.reset(new RobotDrive(1, 2, 3, 4));
		myRobot->SetExpiration(0.1);
		lw = LiveWindow::GetInstance();
		stick0.reset(new Joystick(0));
		// leftStick.reset(new Joystick(LEFT_JOYSTICK_PORT));
		// rightStick.reset(new Joystick(RIGHT_JOYSTICK_PORT));
		controller.reset(new PS3Controller(stick0.get()));
		monitor.reset(new ControllerMonitor<PS3Controller>(*controller));
	}

	void AutonomousInit()
	{
		autoLoopCounter = 0;
		forward = !forward;
	}

	void AutonomousPeriodic()
	{
		if (autoLoopCounter < 50) {
			myRobot->Drive((forward ? -1 : +1) * 0.25, 0.0);
			++autoLoopCounter;
		}
		else {
			myRobot->Drive(0, 0);
		}
	}

	void TeleopInit()
	{
		// Make sure that the robot is stopped!
		myRobot->Drive(0, 0);
		const bool haveLeft = leftStick.get() != nullptr;
		const bool haveRight = rightStick.get() != nullptr;

		if (haveLeft && haveRight) {
			std::cout << "Tank mode enabled...." << std::endl;
		} else if (haveLeft) {
			std::cout << "Arcade mode enabled (left)...." << std::endl;
		} else if (haveRight) {
			std::cout << "Arcade mode enabled (right)...." << std::endl;
		}
	}

	static void ReportJoystickData(Joystick& stick, const std::string& label) {
		std::cout << std::setprecision(3) << std::boolalpha
				  << label << ": "
				  << "x: " << stick.GetX()
				  << ", y: " << stick.GetY()
				  << ", z: " << stick.GetZ()
				  << ", twist: " << stick.GetTwist()
				  << ", throttle: " << stick.GetThrottle()
				  << ", trig: " << stick.GetTrigger()
				  << ", top: " << stick.GetTop()
				  << std::endl;
	}

	void TeleopPeriodic()
	{
		const bool haveZero = stick0.get() != nullptr;
		const bool haveLeft = leftStick.get() != nullptr;
		const bool haveRight = rightStick.get() != nullptr;

		if (monitor.get()) {
			monitor->strobe();
		}
		else if (controller.get()) {
			bool printed = false;
			for (unsigned int i = 0; i < controller->getNumButtons(); ++i ) {
				const PS3Controller::Button button = PS3Controller::Button(i);
				if (controller->isButtonDown(button)) {
					std::cout << std::setw(2) << i
							  << ' '
							  << '(' << controller->getButtonName(button) << ')'
							  << ' ';
					printed = true;
				}
			}
			if (printed) {
				std::cout << std::endl;
			}
		}
		else if (haveZero) {
			this->ReportJoystickData(*stick0, "zero");
		}
		if (haveLeft) {
			this->ReportJoystickData(*leftStick, "left");
		}
		if (haveRight) {
			this->ReportJoystickData(*rightStick, "right");
		}

		/*
		if (haveLeft && haveRight) {
			myRobot->TankDrive(*leftStick, *rightStick);
		} else if (haveLeft) {
			myRobot->ArcadeDrive(*leftStick);		// drive with arcade style: use left stick
		} else if (haveRight) {
			myRobot->ArcadeDrive(*rightStick);		// drive with arcade style: use right stick
		}
		else {
			assert(false);
		}
		*/
	}

	void TestPeriodic()
	{
		lw->Run();
	}
};

START_ROBOT_CLASS(Robot);
